from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
import os

ARGUMENTS = [
    DeclareLaunchArgument('use_sim_time', default_value='true', choices=['true', 'false'], description='Use sim time'),
    DeclareLaunchArgument('namespace', default_value='""', description='Ignition model name'),
    DeclareLaunchArgument('world', default_value='empty_world', description='World name')
]

def generate_launch_description():
    namespace = LaunchConfiguration('namespace')
    use_sim_time = LaunchConfiguration('use_sim_time')

    bridge_params = os.path.join(get_package_share_directory('agv_gazebo'), 'config', 'gz_bridge1.yaml')

    # # cmd_vel bridge
    # cmd_vel_bridge = Node(package='ros_ign_bridge', executable='parameter_bridge',
    #                       name='cmd_vel_bridge',
    #                       output='screen',
    #                       parameters=[{
    #                           'use_sim_time': use_sim_time
    #                       }],
    #                       arguments=[
    #                           '/cmd_vel' + '@geometry_msgs/msg/Twist' + '@ignition.msgs.Twist',
    #                           ['/model/', LaunchConfiguration('robot_name'), '/cmd_vel' +
    #                            '@geometry_msgs/msg/Twist' +
    #                            ']ignition.msgs.Twist']
    #                       ],
    #                       remappings=[
    #                           (['/model/', LaunchConfiguration('robot_name'), '/cmd_vel'],
    #                            'cmd_vel')
    #                       ])

    # Pose bridge
    pose_bridge = Node(package='ros_ign_bridge', executable='parameter_bridge',
                       name='pose_bridge',
                       output='screen',
                       parameters=[{
                            'use_sim_time': use_sim_time
                       }],
                       arguments=[
                           ['/model/', LaunchConfiguration('robot_name'), '/pose' +
                            '@tf2_msgs/msg/TFMessage' +
                            '[ignition.msgs.Pose_V'],
                           ['/model/', LaunchConfiguration('robot_name'), '/pose_static' +
                            '@tf2_msgs/msg/TFMessage' +
                            '[ignition.msgs.Pose_V'],
                       ],
                       remappings=[
                           (['/model/', LaunchConfiguration('robot_name'), '/pose'],
                            'sim_ground_truth_pose'),
                           (['/model/', LaunchConfiguration('robot_name'), '/pose_static'],
                            'sim_ground_truth_pose_static')
                       ])

    # tf bridge
    odom_base_tf_bridge = Node(package='ros_ign_bridge', executable='parameter_bridge',
                               name='odom_base_tf_bridge',
                               output='screen',
                               parameters=[{
                                   'use_sim_time': use_sim_time
                               }],
                               arguments=[
                                   ['/model/', LaunchConfiguration('robot_name'), '/tf' +
                                    '@tf2_msgs/msg/TFMessage' +
                                    '[ignition.msgs.Pose_V']
                               ],
                               remappings=[
                                   (['/model/', LaunchConfiguration('robot_name'), '/tf'], '/tf')
                               ])
    
    gz_bridge_node = Node(package='ros_gz_bridge',
                          executable='parameter_bridge',
                        #   namespace=namespace,
                          name='ign_bridge',
                          parameters=[{'use_sim_time': use_sim_time}, {'config_file': bridge_params}],
                          output='screen')

    image_bridge_node = Node(package='ros_gz_image',
                             executable='image_bridge',
                             arguments=['/depth_camera/image', '/depth_camera/depth_image', '/camera/image'],
                             output='screen')

    ld = LaunchDescription(ARGUMENTS)
    ld.add_action(image_bridge_node)
    ld.add_action(gz_bridge_node)
    # ld.add_action(cmd_vel_bridge)
    ld.add_action(pose_bridge)
    ld.add_action(odom_base_tf_bridge)
    return ld